%% 平面2R机器人非线性力矩计算函数，仅考虑了两关节参数均相同的特殊情况
function Tq_NL = P2R_Robot_Nonlinear_Inv_Dynamic(m,g,L,N,J_theta, J_omega, J_epsilon)
    %% 计算驱动空间状态变量
    M_theta = J_theta .* N; % 电机转角
    M_omega = J_omega .* N; % 电机转速
    M_epsilon = J_epsilon .* N; % 电机角加速度
    
    %% 非线性惯性力矩的求解
    % 非线性惯量矩阵
    Delta_M11 = (2*m*L^2)/N^2 * cos(J_theta(2,1));
    Delta_M22 = 0;
    M12 = (m*L^2)/N^2 * (1+cos(J_theta(2,1)));
    M21 = (m*L^2)/N^2 * (1+cos(J_theta(2,1)));
    Delta_Mm = [Delta_M11 M12; M21 Delta_M22]; 
    % 非线性惯性力矩
    Tq_Delta_Mm = Delta_Mm * M_epsilon;
    
    %% 速度耦合力矩的求解
    % 速度耦合矩阵
    Vm_V12 = -(m*L^2*(2*J_omega(1,1)+J_omega(2,1)))/N^2 * sin(J_theta(2,1));
    Vm_V21 = (m*L^2*J_omega(1,1))/N^2 * sin(J_theta(2,1));
    Vm = [0 Vm_V12; Vm_V21 0];
    % 速度耦合力矩
    Tq_Vm = Vm * M_omega;
    
    %% 重力矩
    Tq_Gm1 = m*g*L/N * (2*cos(J_theta(1,1))+cos(J_theta(1,1)+J_theta(2,1)));
    Tq_Gm2 = m*g*L/N * cos(J_theta(1,1)+J_theta(2,1));
    Tq_Gm = [Tq_Gm1; Tq_Gm2];
    
    %% 当前关节电机的理论非线性力矩
    Tq_NL = Tq_Delta_Mm + Tq_Vm + Tq_Gm; 
    
end